import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;


public class Orientation extends Position {
	private double w;

	public Orientation(double w) {
		super();
		this.w = w;
	}

	public Orientation(double x, double y, double z, double w) {
		super(x, y, z);
		this.w = w;
	}

	public double getW() {
		return w;
	}

	public void setW(double w) {
		this.w = w;
	}

	@Override
	public String toString() {
		return "Orientation [" + super.toString() + " w=" + w + "]";
	}
	
	/**
	 * @param o
	 * @return a vector containing rotation angle about x,y,z
	 */
	public Vector3D convertQuarternionToEulerAngle(){
		double phi = Math.atan2(
						2*(this.getW()*this.getX() + this.getY()*this.getZ()), 
						1 - 2*(Math.pow(this.getX(), 2) + Math.pow(this.getY(), 2))
					);
		double theta = Math.asin(2 * (this.getW()*this.getY() - this.getZ()*this.getX()));
		double psi = Math.atan2(
						2*(this.getW()*this.getZ() + this.getX()*this.getY()), 
						1 - 2*(Math.pow(this.getY(), 2) + Math.pow(this.getZ(), 2))
					);
		return new Vector3D(phi, theta, psi);
	}
	
}
